
#include <VirtualRobot/RuntimeEnvironment.h>
#include <VirtualRobot/Import/RobotImporterFactory.h>

#include <Inventor/Qt/SoQt.h>
#include <QFileInfo>

#include <boost/extension/shared_library.hpp>
#include <boost/function.hpp>
#include <boost/shared_ptr.hpp>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/algorithm/string.hpp>
#include <string>
#include <map>
#include <iostream>
#include <fstream>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <MMM/Motion/Legacy/LegacyMotionReaderXML.h>
#include <MMM/Motion/Legacy/LegacyMotionReaderC3D.h>
#include <MMM/Model/ModelReaderXML.h>
#include <MMM/Model/ModelProcessor.h>
#include <MMM/Model/ModelProcessorFactory.h>
#include <MMM/Motion/Legacy/Converter/ConverterFactory.h>

#include <iostream>
#include <fstream>
#include <assert.h>

#include <VirtualRobot/RobotNodeSet.h>
#include <MMMSimoxTools/MMMSimoxTools.h>
#include "diffmmmConfiguration.h"

using std::cout;
using std::endl;
using namespace VirtualRobot;


static void setRobotConfig(VirtualRobot::RobotPtr robot, VirtualRobot::RobotNodeSetPtr nodeset, MMM::LegacyMotionPtr motion, int frame)
{
    MMM::MotionFramePtr mframe = motion->getMotionFrame(frame);
    if (!mframe)
    {
        std::cout << "internal error" << endl;
        return;
    }
    robot->setGlobalPose(mframe->getRootPose());
    nodeset->setJointValues(mframe->joint);
}

int main(int argc, char *argv[])
{
    DiffmmmConfiguration c;
    cout << " --- MMM motion difference tool --- " << endl;
    if ((argc==2) && (std::string(argv[1]).compare("--help") == 0) )
    {
        std::cout << "Shows the difference in two motion files, which contain the same motion." << std::endl;
        std::cout << "Input Parameters can be [either VICON files (c3d) or] MMM files (xml)." << std::endl;
        std::cout << "The files should contain the same motion, since this tool is used to evaluate "
                << "the difference in motions resulting from either different converters, or "
                << "different approaches." << std::endl;
        c.printSyntax();
        return 0;
    }
    if ((argc<=2))
    {
        c.printSyntax();
        return -1;
    }
    if (!c.processCommandLine(argc,argv))
    {
        std::cout << "Error while processing command line, aborting..." << std::endl;
        c.  printSyntax();
        return -1;
    }
    c.print();
    // input file 1
    QFileInfo fileInfo1(c.file1.c_str());
    //std::string path1(fileInfo1.path().toAscii());
    //std::string filename1(fileInfo1.fileName().toAscii());
    std::string suffix1(fileInfo1.suffix().toLatin1());
    // input file 2
    QFileInfo fileInfo2(c.file2.c_str());
    //std::string path2(fileInfo2.path().toAscii());
    //std::string filename2(fileInfo2.fileName().toAscii());
    std::string suffix2(fileInfo2.suffix().toLatin1());
    // transform suffixes to lowerCase
    std::transform(suffix1.begin(), suffix1.end(), suffix1.begin(), ::tolower);
    std::transform(suffix2.begin(), suffix2.end(), suffix2.begin(), ::tolower);
    // For now, we just compare MMM motion files, so we check for correct suffixes
    if ((suffix1.compare("xml")!=0) || (suffix2.compare("xml")!=0))
    {
        cout << "Expected MMM Motion files as input (.xml). Aborting..." << endl;
        return -1;
    }
#ifdef false
    cout << "Path to Files are [" << path1 << "] and ["  << path2 << "]" << endl;
    cout << "Filenames are [" << filename1 << "] and [" << filename2 << "]" << endl;
    cout << "Suffixes are [" << suffix1 << "] and [" << suffix2 << "]" << endl;
#endif

    // load input files
    MMM::LegacyMotionReaderXMLPtr r(new MMM::LegacyMotionReaderXML());
    // cout << "Checking motions in file 1: " << endl;
    std::vector < std::string > motionNames1 = r->getMotionNames(c.file1);
    // cout << "Checking motions in file 2: " << endl;
    std::vector < std::string > motionNames2 = r->getMotionNames(c.file2);
    MMM::LegacyMotionPtr motion1, motion2; // = r->loadMotion(c.file1,motionNames1[i]);
    // get first motion in motion file or a motion selected via input parameter
    if (motionNames1.size() == 1)
    {
        // if there is only one motion, there is no need to specify it
        motion1 = r->loadMotion(c.file1, motionNames1[0]);
        std::cout << "Motion in file1 found: [" << motionNames1[0] << "@" << c.file1 << "] - Number of frames: " << motion1->getNumFrames() << std::endl;
    } else if (c.motion1specified)
    {
        // search for specified motion
        motion1 = r->loadMotion(c.file1, c.motionName1);
        // output if found, abort if not found
        if (!motion1)
        {
            std::cerr << "Specified motion not found! Available motions are: " << std::endl;
            for (unsigned int i=0; i<motionNames1.size(); i++)
                std::cerr << "\t" << motionNames1[i] << std::endl;
            std::cerr << std::endl;
            return -1;
        } else
        {
            //std::cout << "Specified motion [" << motion1->getName() << "@" << c.file1 << "] found! Number of Frames: " << motion1->getNumFrames() << std::endl;
        }
    } else
    {
        // error => abort
        MMM_ERROR << "Error while loading motion!" << std::endl;
        std::cout << "This may be due to multiple motions in motion file. In this case please specify the motion"
                  << " name with the --motionname1 parameter. Available motion names in this file are: " << std::endl;
        for (unsigned int i=0; i<motionNames1.size(); i++)
            std::cout << "\t" << motionNames1[i] << std::endl;
        std::cout << std::endl;
        std::cout << "Choosing [" << motionNames1[0] << "] as input motion!" << std::endl;
        c.motion1specified = true;
        c.motionName1 = motionNames1[0];
        motion1 = r->loadMotion(c.file1, c.motionName1);
        //return -1;
    }
    if (motionNames2.size() == 1)
    {
        // if there is only one motion, there is no need to specify it
        motion2 = r->loadMotion(c.file2, motionNames2[0]);
        //std::cout << "Motion in file2 found: [" << motionNames2[0] << "@" << c.file2 << "] - Number of frames: " << motion2->getNumFrames() << std::endl;
    } else if (c.motion2specified)
    {
        // search for specified motion
        motion2 = r->loadMotion(c.file2, c.motionName2);
        // output if found, abort if not found
        if (!motion2)
        {
            std::cout << "Specified motion not found! Available motions are: " << std::endl;
            for (unsigned int i=0; i<motionNames2.size(); i++)
                std::cout << "\t" << motionNames2[i] << std::endl;
            std::cout << std::endl;
            return -1;
        } else
        {
            std::cout << "Specified motion [" << motion2->getName() << "@" << c.file2 << "] found! Number of Frames: " << motion2->getNumFrames() << std::endl;
        }
    } else
    {
        // error => abort
        MMM_ERROR << "Error while loading motion!" << std::endl;
        std::cout << "This may be due to multiple motions in motion file. In this case please specify the motion"
                  << " name with the --motionname2 parameter. Available motion names in this file are: " << std::endl;
        for (unsigned int i=0; i<motionNames2.size(); i++)
            std::cout << "\t" << motionNames2[i] << std::endl;
        std::cout << std::endl;
        std::cout << "Choosing [" << motionNames2[0] << "] as input motion!" << std::endl;
        c.motion2specified = true;
        c.motionName2 = motionNames2[0];
        motion2 = r->loadMotion(c.file2, c.motionName2);
        //return -1;
    }
    // Compare number of frames and save the highest frame number to compare
    unsigned int numFrames = motion1->getNumFrames();
    if (numFrames != motion2->getNumFrames())
    {
        if (numFrames > motion2->getNumFrames())
            numFrames = motion2->getNumFrames();
        std::cout << "Number of motion frames don't match. Motion is compared only up till frame [" << numFrames << "]" << std::endl;
    }
    // Load models
    MMM::ModelPtr model1 =  motion1->getModel(false);
    MMM::ModelPtr model2 =  motion2->getModel(false);
    // Compare used models
    if (model1->getFilename() != model2->getFilename())
    {
        std::cout << "Model names don't match! May be a false positive if directories don't match!" << std::endl;
        std::cout << "Model1: [" << model1->getFilename() << "]" << std::endl;
        std::cout << "Model2: [" << model2->getFilename() << "]" << std::endl;
    }
    // Create two VirtualRobot::Robot
    // -> redirect error console to get rid of annoying debug messages
    std::streambuf* errorConsole = std::cerr.rdbuf();       // save error console
    std::stringstream ss;
    std::cerr.rdbuf(ss.rdbuf());                            // redirect error console
    VirtualRobot::RobotPtr robot1 = MMM::SimoxTools::buildModel( model1, false);
    VirtualRobot::RobotPtr robot2 = MMM::SimoxTools::buildModel( model2, false);
    MMM::SimoxTools::updateInertialMatricesFromModels(robot1);
    MMM::SimoxTools::updateInertialMatricesFromModels(robot2);
    VirtualRobot::RobotNodeSetPtr rns1;
    VirtualRobot::RobotNodeSetPtr rns2;
    std::vector<std::string> jointNames1 = motion1->getJointNames();
    std::vector<std::string> jointNames2 = motion2->getJointNames();
    if (jointNames1.size()>0)
    {
        std::string rnsName("MMMViewerRNS");
        if (robot1->hasRobotNodeSet(rnsName))
            robot1->deregisterRobotNodeSet(robot1->getRobotNodeSet(rnsName));
        rns1 = VirtualRobot::RobotNodeSet::createRobotNodeSet(robot1, rnsName, jointNames1, "", "", true);
    }
    if (jointNames2.size()>0)
    {
        std::string rnsName("MMMViewerRNS");
        if (robot2->hasRobotNodeSet(rnsName))
            robot2->deregisterRobotNodeSet(robot2->getRobotNodeSet(rnsName));
        rns2 = VirtualRobot::RobotNodeSet::createRobotNodeSet(robot2, rnsName, jointNames2, "", "", true);
    }
    std::cerr.rdbuf(errorConsole);                          // restore error console
    // improve performance
//    robot1 = MMM::SimoxTools::buildReducedModel(robot1, jointNames1);
//    robot1->setUpdateCollisionModel(false);
//    robot1->setUpdateVisualization(false);
//    robot1->setupVisualization(false, false);
//    robot1->setThreadsafe(false);
//    robot2 = MMM::SimoxTools::buildReducedModel(robot2, jointNames2);
//    robot2->setUpdateCollisionModel(false);
//    robot2->setUpdateVisualization(false);
//    robot2->setupVisualization(false, false);
//    robot2->setThreadsafe(false);

    // Skalieren der VirtualRobot::Roboter entsprechend den Werten in der Bewegungsdatei ?!?

    // extract marker names
    std::vector<SensorPtr> sensorList1 =  robot1->getSensors();
    std::vector<SensorPtr> sensorList2 =  robot2->getSensors();
    /*
    std::cout << "SensorList for robot1:" << std::endl << "[";
    for (int i=0; i<sensorList1.size(); i++)
    {
        std::cout << sensorList1[i]->getName();
        if (i!=sensorList1.size()-1)
            std::cout << ", ";
        else
            std::cout << "]" << std::endl;
    }
    std::cout << "SensorList for robot2:" << std::endl << "[";
    for (int i=0; i<sensorList2.size(); i++)
    {
        std::cout << sensorList2[i]->getName();
        if (i!=sensorList2.size()-1)
            std::cout << ", ";
        else
            std::cout << "]" << std::endl;
    }
    */
    // create logging containers
    std::vector< std::vector<Eigen::Vector3f> > allMarkerDeltas;                    // per marker, per frame (3D)
    std::vector< std::vector<float> >           allMarkerDiffs;                     // per marker, per frame (1D)
    std::vector<float>                          markerAvg, markerMin, markerMax;    // per marker
    std::vector<float>                          avgDelta, minDelta, maxDelta;       // per frame
    // todo: check consistency (do marker_index1 -> marker_index2 belong to the same marker?)
    // *** iterate over whole motion and compute difference vectors and min, max and avg values
    Eigen::Vector3f x1, x2, delta;
    float min, max, avg, diff, sum, totalAvg, totalMin, totalMax;
    float minMarker = totalMin = std::numeric_limits<float>::infinity();
    float maxMarker = totalAvg = totalMax = 0.0f;
    // prefill marker information
    for (unsigned int sensorCounter=0; sensorCounter<sensorList1.size(); sensorCounter++)
    {
        markerMin.push_back(minMarker);
        markerAvg.push_back(0.0f);
        markerMax.push_back(maxMarker);
    }
    // iterate over whole motion
    for (unsigned int frameCounter=0; frameCounter<numFrames; frameCounter++)
    {
        // set motion frame in robot models
        setRobotConfig(robot1, rns1, motion1, frameCounter);
        setRobotConfig(robot2, rns2, motion2, frameCounter);
        std::vector<Eigen::Vector3f> markerDelta;
        std::vector<float>           markerDiff;
        min = std::numeric_limits<float>::infinity();
        max = sum = diff = 0;
        for (unsigned int sensorCounter=0; sensorCounter<sensorList1.size(); sensorCounter++)
        {
            x1 = sensorList1[sensorCounter]->getGlobalPose().block(0,3,3,1);
            x2 = sensorList2[sensorCounter]->getGlobalPose().block(0,3,3,1);
            delta = x2 - x1;
            markerDelta.push_back(delta);
            diff = delta.norm();
            markerDiff.push_back(diff);
            // evaluate frames
            if (diff<min)
                min = diff;
            if (diff>max)
                max = diff;
            sum += diff;
            // evaluate markers
            if (diff<markerMin[sensorCounter])
                markerMin[sensorCounter] = diff;
            if (diff>markerMax[sensorCounter])
                markerMax[sensorCounter] = diff;
            markerAvg[sensorCounter] += diff;
        }
        avg = sum / sensorList1.size();
        allMarkerDeltas.push_back(markerDelta);
        allMarkerDiffs.push_back(markerDiff);
        avgDelta.push_back(avg);
        minDelta.push_back(min);
        maxDelta.push_back(max);
        totalAvg += avg;
        if (min<totalMin)
            totalMin = min;
        if (max>totalMax)
            totalMax = max;
    }
    totalAvg /= numFrames;
    for (unsigned int sensorCounter=0; sensorCounter<sensorList1.size(); sensorCounter++)
        markerAvg[sensorCounter] /= numFrames;
    // *** create output according to command line parameters (for now there's just one version)
    std::streambuf* outConsole = std::cout.rdbuf();       // save console
    std::ofstream fout;
    fout.open(c.outputFile.c_str(), std::ofstream::out);
    if (fout && c.outputFileSpecified)
        std::cout.rdbuf(fout.rdbuf());
    // redirect if outputfile defined
    // std::cout.rdbuf(ss.rdbuf());                       // redirect output console?
    switch (c.diffMode) {
    case DiffmmmConfiguration::eOutputDetailed:
        // output per frame
        std::cout << "Frame; avg; min; max" << std::endl;
        std::cout << "total; " << totalAvg << "; " << totalMin << "; " << totalMax << std::endl;
        for (unsigned int frameCounter=0; frameCounter<numFrames; frameCounter++)
            std::cout << frameCounter << "; " << avgDelta[frameCounter] << "; " << minDelta[frameCounter] << "; " << maxDelta[frameCounter] << std::endl;
        std::cout << std::endl;
        // TODO: output for every marker
        std::cout << "Frame";
        for (unsigned int i=0; i<sensorList1.size(); i++)
            std::cout << "; " << sensorList1[i]->getName();
        std::cout << std::endl;
        for (unsigned int frameCounter=0; frameCounter<numFrames; frameCounter++)
        {
            std::cout << frameCounter;
            for (unsigned int i=0; i<sensorList1.size(); i++)
                std::cout << "; " << (allMarkerDiffs[frameCounter])[i];
            std::cout << std::endl;
        }
        break;
    case DiffmmmConfiguration::eOutputOverviewOnly:
        std::cout << "Total avg marker errors: " << totalAvg << std::endl;
        std::cout << "Avg marker error per frame: [";
        for (unsigned int frameCounter=0; frameCounter<numFrames-1; frameCounter++)
            std::cout << avgDelta[frameCounter] << ", ";
        std::cout << avgDelta[numFrames-1] << "]" << std::endl;
        std::cout << "min/max marker error per frame: [";
        for (unsigned int frameCounter=0; frameCounter<numFrames-1; frameCounter++)
            std::cout << "(" << minDelta[frameCounter] << "/" << maxDelta[frameCounter] << "), ";
        std::cout << "(" << minDelta[numFrames-1] << "/" << maxDelta[numFrames-1] << ")]" << std::endl;
        break;
    case DiffmmmConfiguration::eOutputAvgOnly:
        std::cout << "total marker errors avg/min/max: " << totalAvg << ";" << totalMin << ";" << totalMax << std::endl;
        break;
    default:
        std::cout << "Unknown diff mode or not implemented yet!" << std::endl;
        break;
    }
    std::cout.rdbuf(outConsole);                          // restore output console
    if (fout)
        fout.close();
    return 0;
}

